#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>	
#include <sys/time.h>

using namespace std;
using namespace PlayerCc;
FILE *posoutput;
FILE *posoutput1;

//goto definitions
double Xdest,Ydest;
bool finish = false;

int main(int argc, char** argv){
Xdest=-3;
Ydest=-3;
	
posoutput=fopen("odoDataGPS.txt","w");
posoutput1=fopen("odoDataERR.txt","w");
	
//These are initialised before the infinite loop
PlayerClient robot(gHostname, gPort);	//This is needed in every player client application
Position2dProxy pp(&robot, gIndex);	//This is needed in every player client application


double x,y,yaw,oldx,oldy,oldyaw;	//Variables used to find change in displacement d_d and change in orientation d_t
robot.Read();				//Obtain DR and laser data
oldx=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
oldy=pp.GetYPos();
oldyaw=pp.GetYaw();


double xest=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
double yest=pp.GetYPos();
double yawest=pp.GetYaw();
	int i=0;

//error def

float Dx=0;
float Dy=0;


while(!finish){
	i=i+1;

try{

// errore odometrico viene aggiunto ad ogni passo e dovrebbe aumentare (sommarsi) non funziona
 
	
//These are called during the inifinite recursive loop
robot.Read();				//Obtain DR and laser data
x=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
y=pp.GetYPos();
yaw=pp.GetYaw();
	
                double cx=x-oldx;
		double cy=y-oldy;
		double d_d=sqrt(cx*cx+cy*cy);
		double d_t=yaw-oldyaw;


//   float Dxi=srand(1) %100;
//   float Dyi=srand(1) %100;
float Dxi= 2.0f * ((float)rand() / (float)RAND_MAX) - 1.0f;
float Dyi= 2.0f * ((float)rand() / (float)RAND_MAX) - 1.0f ;
	Dxi=Dxi*0.1;
	Dyi=Dyi*0.1;

 	
Dx=Dx+Dxi;
Dy=Dy+Dyi;
     printf(" Derrore : [%f, %f]\n ",Dxi,Dyi );
     printf(" errore : [%f, %f]\n ",Dx,Dy );



xest=oldx+Dx;
yest=oldy+Dy;
yawest= oldyaw;	
	//xest=oldx+ (d_d+n)*cos(d_t+n);
 	//yest=oldy+ (d_d+n)*sin(d_t+n);
 	//yawest= oldyaw +d_t;	


//logdata
	
fprintf(posoutput,"%f %f %f\n",x,y,yaw);
fprintf(posoutput1,"%f %f %f\n",xest,yest,yawest);

oldx=x;
oldy=y;
oldyaw=yaw;

// goto commands . . . . .  . . . .. . . . .  . . . . 
	 robot.Read();
     pp.GoTo(Ydest,Xdest,0);

// distance from target

float distT = sqrt((x-Xdest)*(x-Xdest)+(y-Ydest)*(y-Ydest));


if(distT<0.1){
finish= true;
}

}
  catch (PlayerCc::PlayerError & e)
  {
    std::cerr << e << std::endl;
    return -1;
  }

}//THE END loop


fclose(posoutput);
fclose(posoutput1);

}
	


